// Chris Bena
// CPEG 467 - ERA MOBI Robot project
// 10/13/2010

/*
 * Drive around by hugging walls.  Should be able to traverse a maze.
 * Hugs the right wall.
 * 
 * This program uses functions for turning 90 degrees and corners
 */

#include <libplayerc++/playerc++.h>
#include <iostream>
#include <fstream>

#include "args.h"

#define FRONT 327
#define RIGHT 110
#define LEFT 546
#define MIN_RANGE 0.5 // min range on the right
#define MAX_RANGE 1.0 // max range on the right
#define COLLISION 0.35 // the min range for collision detection

// max range set to 1.5 for stage; on robot it 
// should be about 0.8-1.0

using namespace PlayerCc;
    
PlayerClient robot(gHostname, gPort);
Position2dProxy pp(&robot, gIndex);
LaserProxy lp(&robot, gIndex);
std::ofstream fout;

void turn(double newspeed, double newturnrate){
  pp.SetSpeed(newspeed, newturnrate);
}

// Turns right - turns until lp[FRONT] is maxed out;
void turn(){
  do{
    //pp.SetSpeed(0.1, -0.75);
    pp.SetSpeed(0.0, 0.375);
    robot.Read();
  }
  while(lp[FRONT] < 5.58);
}

void saveData(){
  double x = pp.GetXPos();
  double y = pp.GetYPos();
  double yaw = pp.GetYaw();

  //fout << "X: "; 
  //fout.width(13); 
  fout << x; 
  fout << " ";
  //fout << "Y: ";
  //fout.width(13);
  fout << y;
  fout << " ";
  //fout << "Yaw: ";
  //fout.width(13);
  fout << rtod(yaw);
  fout << " ";
  //fout << "Right: "; 
  fout << lp[RIGHT];
  fout << " ";
  //fout << "Left: ";
  fout << lp[LEFT] << "\n";
}

int main(int argc, char **argv)
{
  parse_args(argc,argv);

  fout.open("OutputData.txt");

  if(!fout.is_open()){
    std::cerr << "Error opening file\n";
    exit(-1);
  }

  fout.setf(std::ios::fixed, std::ios::floatfield);
  fout.precision(8);

  // we throw exceptions on creation if we fail
  try
  {
    pp.SetMotorEnable (true);
    
    double newspeed = 0;
    double newturnrate = 0;
    //double minR = 5.6;
    //double minL = 5.6;

    robot.Read(); //here so GetRange doesn't segfault
      
    // go into read-think-act loop
    for(;;)
    {

      // this blocks until new data comes; 10Hz by default
      robot.Read();

      /* Using only the certain sensors - front, left, right */
      double front = lp[FRONT];
      double frontRight = lp[218];
      double frontLeft = lp[437];
      double right = lp[RIGHT];
      double left = lp[LEFT];
      double yaw = pp.GetYaw();
      double minRight = lp.GetMinRight();
      double minDistRight=5.6;
      double minDistLeft=5.6;

      for(int i = RIGHT; i <= FRONT; i++){
	if(lp[i] < minDistRight)
	  minDistRight=lp[i];
      }
      for(int i = FRONT; i <= LEFT; i++){
	if(lp[i] < minDistLeft)
	  minDistLeft=lp[i];
      }

      // if inside range
      if(frontRight <= MIN_RANGE || minDistRight <= COLLISION){
	//std::cout << "inside of range\n";
	newspeed = 0.0;
	newturnrate = 0.175;
	turn(newspeed, newturnrate);
      }
      
      // else if outside range
      else if(frontRight > MAX_RANGE || minDistLeft <= COLLISION){
        //std::cout << "Outside range\n";
        //std::cout << frontRight << "\n";
        newspeed = 0.25;
        newturnrate = -0.190;
        turn(newspeed, newturnrate);
      }

      else{
	//std::cout << "Straight ahead\n";
	newspeed = 0.5;
	newturnrate = 0.0;
	turn(newspeed, newturnrate);
      }

      // if reached a corner
      if(front <= 1.5 && frontRight <= 0.8){
        //std::cout << "Turning a corner\n";
        turn();
      }

      // when we test for front <= 1.0 it gets very close to the wall 
      // and then starts to turn.  The reason is that there's a while
      // between when the condition turns true and the robot responds
      // to the new turn command.

      saveData();

    }
  }

  catch (PlayerCc::PlayerError e)
  {
    std::cerr << e << std::endl;
    return -1;
  }
}
